/*
 * PI_REG.c
 *
 * Created: 24.10.2012 17:31:05
 *  Author: mariuvi
 */ 

#include <avr/eeprom.h>
#include <stdlib.h>


#define D_max 40
#define FEEDFORWARD 18

static uint8_t EEMEM P_gain_EEMEM = 5;
static uint8_t EEMEM D_gain_EEMEM = 10;
static uint8_t P_gain = 0;
static uint8_t D_gain = 0;
static int error_prev = 0;


uint8_t REG_read_P()
{
	return P_gain;
}

uint8_t REG_read_D()
{
	return D_gain;
}

void REG_param_save(uint8_t P, uint8_t D)
{
	eeprom_write_byte(&P_gain_EEMEM, P);
	eeprom_write_byte(&D_gain_EEMEM, D);
	P_gain = P;
	D_gain = D;				
}

void REG_param_load()
{
	P_gain = eeprom_read_byte(&P_gain_EEMEM);
	D_gain = eeprom_read_byte(&D_gain_EEMEM);
}


int REG_controller(int error, float dt)
{
	int p, d, sign, u;
	
	//Finds the sign of the error
	if(error < -1)
	{
		sign = -1;
	}
	else if(error > 1)
	{
		sign = 1;
	}
	else
	{
		sign = 0;
	}
	
	//Calculate P 
	p = 0.1*P_gain*error;
	
	//Calculate D, with saturation
	if(dt != 0)
	{
		d = 0.0001*D_gain*(error - error_prev)/dt;	
	}
	if(d > D_max)
	{
		d = D_max;
	}
	else if(d < -D_max)
	{
		d = -D_max;
	}	
	error_prev = error;
	
	u = p + d + sign*FEEDFORWARD;
	
	return u;
}